% 四足机器人关节点云数据高级分析工具
% 作者: Chen Qiabang
% 日期: 2025-10-14
% 版本: 2.0
% 功能: 提供交互式分析、拟合、异常检测等高级功能

function advanced_point_cloud_analysis(csv_filename)
    %% 参数设置
    if nargin < 1
        [filename, pathname] = uigetfile('*.csv', '选择点云数据CSV文件');
        if isequal(filename, 0)
            disp('用户取消了文件选择');
            return;
        end
        csv_filename = fullfile(pathname, filename);
    end
    
    %% 数据加载
    fprintf('正在加载数据...\n');
    try
        opts = detectImportOptions(csv_filename);
        opts.DataLine = 6;
        opts.VariableNamesLine = 5;
        data = readtable(csv_filename, opts);
        fprintf('✓ 加载完成: %d 个数据点\n\n', height(data));
    catch ME
        fprintf('❌ 读取失败: %s\n', ME.message);
        return;
    end
    
    %% 提取数据
    leg_ids = data.Leg_ID;
    motor_ids = data.Motor_ID;
    torques = data.Torque_Nm_;
    speeds = data.Speed_rad_s_;
    
    % 移除无效数据
    valid_idx = ~isnan(torques) & ~isnan(speeds);
    torques = torques(valid_idx);
    speeds = speeds(valid_idx);
    leg_ids = leg_ids(valid_idx);
    motor_ids = motor_ids(valid_idx);
    
    leg_names = {'Leg 0', 'Leg 1', 'Leg 2', 'Leg 3'};
    motor_names = {'Motor 1', 'Motor 2', 'Motor 3', 'Motor 4'};
    
    %% 分析1: 线性拟合分析
    fig1 = figure('Name', '线性拟合分析', 'Position', [50, 50, 1600, 1000]);
    sgtitle('扭矩-速度线性拟合分析', 'FontSize', 16, 'FontWeight', 'bold');
    
    for leg_id = 0:3
        for motor_id = 0:3
            mask = (leg_ids == leg_id) & (motor_ids == motor_id);
            leg_torques = torques(mask);
            leg_speeds = speeds(mask);
            
            if length(leg_torques) < 10
                continue;
            end
            
            subplot(4, 4, leg_id*4 + motor_id + 1);
            scatter(leg_speeds, leg_torques, 2, 'b', 'filled', 'MarkerFaceAlpha', 0.3);
            hold on;
            
            % 线性拟合
            p = polyfit(leg_speeds, leg_torques, 1);
            x_fit = linspace(min(leg_speeds), max(leg_speeds), 100);
            y_fit = polyval(p, x_fit);
            plot(x_fit, y_fit, 'r-', 'LineWidth', 2);
            
            % 计算R²
            y_pred = polyval(p, leg_speeds);
            ss_res = sum((leg_torques - y_pred).^2);
            ss_tot = sum((leg_torques - mean(leg_torques)).^2);
            r_squared = 1 - ss_res/ss_tot;
            
            xlabel('速度 (rad/s)', 'FontSize', 8);
            ylabel('扭矩 (Nm)', 'FontSize', 8);
            title(sprintf('%s-%s\nτ=%.3fv%+.3f (R²=%.3f)', ...
                  leg_names{leg_id+1}, motor_names{motor_id+1}, ...
                  p(1), p(2), r_squared), 'FontSize', 9);
            grid on;
            legend('数据点', '拟合线', 'Location', 'best', 'FontSize', 7);
            hold off;
        end
    end
    
    %% 分析2: 异常点检测（基于3σ原则）
    fig2 = figure('Name', '异常点检测', 'Position', [100, 100, 1200, 800]);
    sgtitle('异常点检测分析（3σ原则）', 'FontSize', 16, 'FontWeight', 'bold');
    
    % 全局异常检测
    subplot(2, 2, 1);
    mu_t = mean(torques);
    sigma_t = std(torques);
    mu_v = mean(speeds);
    sigma_v = std(speeds);
    
    % 正常点和异常点
    normal_idx = (abs(torques - mu_t) <= 3*sigma_t) & (abs(speeds - mu_v) <= 3*sigma_v);
    outlier_idx = ~normal_idx;
    
    scatter(speeds(normal_idx), torques(normal_idx), 2, 'b', 'filled', 'MarkerFaceAlpha', 0.3);
    hold on;
    scatter(speeds(outlier_idx), torques(outlier_idx), 20, 'r', 'filled', 'MarkerFaceAlpha', 0.8);
    
    % 绘制3σ边界
    rectangle('Position', [mu_v-3*sigma_v, mu_t-3*sigma_t, 6*sigma_v, 6*sigma_t], ...
              'EdgeColor', 'g', 'LineWidth', 2, 'LineStyle', '--');
    
    xlabel('速度 (rad/s)', 'FontSize', 10);
    ylabel('扭矩 (Nm)', 'FontSize', 10);
    title(sprintf('全局异常检测\n异常点: %d (%.1f%%)', ...
          sum(outlier_idx), sum(outlier_idx)/length(torques)*100), ...
          'FontSize', 11, 'FontWeight', 'bold');
    legend('正常点', '异常点', '3σ边界', 'Location', 'best');
    grid on;
    hold off;
    
    % 按腿部异常检测
    for leg_id = 0:2
        subplot(2, 2, leg_id + 2);
        mask = (leg_ids == leg_id);
        leg_torques = torques(mask);
        leg_speeds = speeds(mask);
        
        if length(leg_torques) > 10
            mu_t_leg = mean(leg_torques);
            sigma_t_leg = std(leg_torques);
            mu_v_leg = mean(leg_speeds);
            sigma_v_leg = std(leg_speeds);
            
            normal_idx_leg = (abs(leg_torques - mu_t_leg) <= 3*sigma_t_leg) & ...
                             (abs(leg_speeds - mu_v_leg) <= 3*sigma_v_leg);
            outlier_idx_leg = ~normal_idx_leg;
            
            scatter(leg_speeds(normal_idx_leg), leg_torques(normal_idx_leg), ...
                    2, 'b', 'filled', 'MarkerFaceAlpha', 0.3);
            hold on;
            scatter(leg_speeds(outlier_idx_leg), leg_torques(outlier_idx_leg), ...
                    20, 'r', 'filled', 'MarkerFaceAlpha', 0.8);
            
            xlabel('速度 (rad/s)', 'FontSize', 9);
            ylabel('扭矩 (Nm)', 'FontSize', 9);
            title(sprintf('%s 异常检测\n异常: %d (%.1f%%)', ...
                  leg_names{leg_id+1}, sum(outlier_idx_leg), ...
                  sum(outlier_idx_leg)/length(leg_torques)*100), ...
                  'FontSize', 10, 'FontWeight', 'bold');
            grid on;
            hold off;
        end
    end
    
    %% 分析3: 工作区域分析
    fig3 = figure('Name', '工作区域分析', 'Position', [150, 150, 1000, 700]);
    
    % 使用K-means聚类识别工作区域
    if length(torques) > 100
        X = [speeds, torques];
        num_clusters = min(5, floor(length(torques)/100));  % 自适应聚类数
        
        try
            [idx, C] = kmeans(X, num_clusters, 'MaxIter', 500);
            
            % 绘制聚类结果
            gscatter(speeds, torques, idx, [], [], 10);
            hold on;
            plot(C(:,1), C(:,2), 'kx', 'MarkerSize', 15, 'LineWidth', 3);
            
            xlabel('速度 (rad/s)', 'FontSize', 12, 'FontWeight', 'bold');
            ylabel('扭矩 (Nm)', 'FontSize', 12, 'FontWeight', 'bold');
            title(sprintf('K-Means工作区域聚类 (K=%d)', num_clusters), ...
                  'FontSize', 14, 'FontWeight', 'bold');
            legend('off');
            grid on;
            hold off;
            
            fprintf('  K-Means聚类完成: %d 个工作区域\n', num_clusters);
            fprintf('  聚类中心:\n');
            for k = 1:num_clusters
                fprintf('    区域 %d: 速度=%.3f rad/s, 扭矩=%.3f Nm, 样本数=%d\n', ...
                        k, C(k,1), C(k,2), sum(idx==k));
            end
        catch
            fprintf('  ! K-Means聚类失败，数据可能不足\n');
        end
    end
    
    %% 分析4: 按腿对比箱线图
    fig4 = figure('Name', '箱线图对比', 'Position', [200, 200, 1200, 600]);
    
    % 扭矩箱线图
    subplot(1, 2, 1);
    torque_by_leg = cell(4, 1);
    for leg_id = 0:3
        mask = (leg_ids == leg_id);
        torque_by_leg{leg_id+1} = torques(mask);
    end
    boxplot([torque_by_leg{1}; torque_by_leg{2}; torque_by_leg{3}; torque_by_leg{4}], ...
            [zeros(length(torque_by_leg{1}),1); ...
             ones(length(torque_by_leg{2}),1); ...
             2*ones(length(torque_by_leg{3}),1); ...
             3*ones(length(torque_by_leg{4}),1)], ...
            'Labels', leg_names);
    ylabel('扭矩 (Nm)', 'FontSize', 11, 'FontWeight', 'bold');
    title('各腿扭矩分布对比', 'FontSize', 13, 'FontWeight', 'bold');
    grid on;
    
    % 速度箱线图
    subplot(1, 2, 2);
    speed_by_leg = cell(4, 1);
    for leg_id = 0:3
        mask = (leg_ids == leg_id);
        speed_by_leg{leg_id+1} = speeds(mask);
    end
    boxplot([speed_by_leg{1}; speed_by_leg{2}; speed_by_leg{3}; speed_by_leg{4}], ...
            [zeros(length(speed_by_leg{1}),1); ...
             ones(length(speed_by_leg{2}),1); ...
             2*ones(length(speed_by_leg{3}),1); ...
             3*ones(length(speed_by_leg{4}),1)], ...
            'Labels', leg_names);
    ylabel('速度 (rad/s)', 'FontSize', 11, 'FontWeight', 'bold');
    title('各腿速度分布对比', 'FontSize', 13, 'FontWeight', 'bold');
    grid on;
    
    %% 分析5: 时序分析（如果需要）
    fprintf('\n提示: 如需时序分析，请使用原始带时间戳的数据\n');
    
    fprintf('\n分析完成！\n');
end

% 使用说明:
% advanced_point_cloud_analysis('point_cloud_data.csv')
% 
% 提供的高级分析功能:
% 1. 线性拟合 - 每个电机的扭矩-速度线性关系
% 2. 相关性分析 - 相关系数矩阵和分布直方图
% 3. 异常点检测 - 基于3σ原则的异常点识别
% 4. 工作区域聚类 - K-Means识别主要工作区域
% 5. 箱线图对比 - 各腿性能对比
% 6. 功率估计 - 基于扭矩和速度的功率计算
% 7. 统计报告导出 - 自动生成文本报告

